﻿using OpenTK;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace LDLib
{
    /// <summary>
    /// A small collection of math-related tools.
    /// </summary>
    public static class MathTools
    {
        /// <summary>
        /// Converts an euler-angle rotation to quaternion rotation.
        /// The rotation is applied in X-Y-Z order.
        /// </summary>
        /// <param name="euler">Euler angle rotation</param>
        /// <returns>Quaternion represention of the rotation</returns>
        public static Quaternion FromEulerAngles(Vector3 euler)
        {
            Vector3 v = new Vector3();
            v.X = MathHelper.DegreesToRadians(euler.X);
            v.Y = MathHelper.DegreesToRadians(euler.Y);
            v.Z = MathHelper.DegreesToRadians(euler.Z);
            return Quaternion.FromAxisAngle(Vector3.UnitX, v.X) * Quaternion.FromAxisAngle(Vector3.UnitY, v.Y) * Quaternion.FromAxisAngle(Vector3.UnitZ, v.Z);
        }

        /// <summary>
        /// Converts a quaternion to euler-angle rotation.
        /// </summary>
        /// <param name="rotation">Rotation to convert</param>
        /// <returns>An euler-angle representation of the rotation</returns>
        public static Vector3 ToEulerAngles(Quaternion rotation)
        {
            Vector3 v = new Vector3();
            v.X = (float)Math.Atan2(2 * (rotation.Y * rotation.Z + rotation.X * rotation.W), 1 - 2 * (Math.Pow(rotation.X, 2) - Math.Pow(rotation.Y, 2)));
            v.Y = (float)Math.Asin(2 * (rotation.X * rotation.Z + rotation.W * rotation.Y));
            v.Z = (float)Math.Atan2(2 * (rotation.X * rotation.Y + rotation.Z * rotation.W), 1 - 2 * (Math.Pow(rotation.Y, 2) - Math.Pow(rotation.Z, 2)));
            v.X = MathHelper.RadiansToDegrees(v.X);
            v.Y = MathHelper.RadiansToDegrees(v.Y);
            v.Z = MathHelper.RadiansToDegrees(v.Z);
            return v;
        }

        /// <summary>
        /// Hacky-wacky matrix decomposition ^_^
        /// Assumes that scale is applied before rotation.
        /// </summary>
        /// <param name="matrix">Matrix to decompose</param>
        /// <param name="position">Out vector for position</param>
        /// <param name="rotation">Out quaternion for rotation</param>
        /// <param name="scale">Out vector for scale</param>
        public static void Decompose(ref Matrix4 matrix, out Vector3 position, out Quaternion rotation, out Vector3 scale)
        {
            Matrix4 m = matrix;
            position = m.Translation;
            m.Translation = Vector3.Zero;

            Vector3 up = Vector3.Transform(Vector3.UnitY, m);
            Vector3 left = Vector3.Transform(Vector3.UnitX, m);
            Vector3 forward = Vector3.Transform(Vector3.UnitZ, m);

            m.M11 /= left.Length;
            m.M22 /= up.Length;
            m.M33 /= forward.Length;

            Quaternion.CreateFromRotationMatrix(ref m, out rotation);
            scale = new Vector3(left.Length, up.Length, forward.Length);
        }

        /// <summary>
        /// Constructs a randomized unit vector
        /// </summary>
        /// <returns>A randomized unit vector</returns>
        public static Vector3 GetRandomVector3()
        {
            Vector3 v = new Vector3((float)Engine.Random.NextDouble(), (float)Engine.Random.NextDouble(), (float)Engine.Random.NextDouble());
            v.Normalize();
            return v;
        }

        /// <summary>
        /// Linear interpolation function
        /// </summary>
        /// <param name="v0">Start value</param>
        /// <param name="v1">End value</param>
        /// <param name="d">Delta value</param>
        /// <returns>A linearly interpolated value</returns>
        public static float Lerp(float v0, float v1, float d)
        {
            return v0 + (v1 - v0) * d;
        }

        /// <summary>
        /// Unprojects a location on the screen
        /// </summary>
        /// <param name="projection">Projection matrix</param>
        /// <param name="view">View matrix</param>
        /// <param name="viewport">Viewport</param>
        /// <param name="mouse">Mouse location</param>
        /// <returns>An unprojected coordinate</returns>
        public static Vector4 UnProject(Matrix4 projection, Matrix4 view, Size viewport, Vector3 mouse)
        {
            Vector4 vec;

            vec.X = 2.0f * mouse.X / (float)viewport.Width - 1;
            vec.Y = -(2.0f * mouse.Y / (float)viewport.Height - 1);
            vec.Z = mouse.Z;
            vec.W = 1.0f;

            Matrix4 viewInv = Matrix4.Invert(view);
            Matrix4 projInv = Matrix4.Invert(projection);

            Vector4.Transform(ref vec, ref projInv, out vec);
            Vector4.Transform(ref vec, ref viewInv, out vec);

            if (vec.W > float.Epsilon || vec.W < float.Epsilon)
            {
                vec.X /= vec.W;
                vec.Y /= vec.W;
                vec.Z /= vec.W;
            }

            return vec;
        }
    }
}
